#include "ros/ros.h"
#include "geometry_msgs/Point.h"
#include "rvo_ros/SetGoals.h"

int main(int argc, char **argv)
{
    // 初始化ROS节点
    std::string iris_id = argv[1]; // '1'
    ros::init(argc, argv, "set_goals_client_" + iris_id);
    ros::NodeHandle n;

    // 创建服务客户端，连接到set_rvo_goals服务
    ros::ServiceClient client = n.serviceClient<rvo_ros::SetGoals>("set_rvo_goals_" + iris_id);

    // 创建服务请求对象
    rvo_ros::SetGoals srv;

    // 从命令行参数获取模型类型（默认或随机）
    srv.request.model = argv[2];
    int args_num = argc - 1;       // 参数总数
    int coordinate_num = argc - 3; // 坐标参数数目

    // 输出输入的参数
    std::cout << "arguments input:" << std::endl;

    // 如果没有输入参数，输出错误信息
    if (args_num == 0)
        ROS_ERROR("Please input the args");

    // 如果坐标数目不是3的倍数，输出错误信息
    if (coordinate_num % 3 != 0)
    {
        std::cout << "The num of coordinates is wrong" << std::endl;
        return 1;
    }

    // 如果模型类型是默认模型，解析坐标点
    if (srv.request.model == "default")
    {
        for (int i = 3; i < coordinate_num + 3; i = i + 3)
        {
            geometry_msgs::Point point;
            point.x = atof(argv[i]);                  // 将字符串转换为浮点数
            point.y = atof(argv[i + 1]);              // 将字符串转换为浮点数
            point.z = atof(argv[i + 2]);              // 将字符串转换为浮点数
            srv.request.coordinates.push_back(point); // 将坐标点添加到请求中
        }
    }


    // 调用服务并处理响应
    if (client.call(srv))
    {
        ROS_INFO("call service successfully, number of goals: %ld", srv.response.num_goal);
    }
    else
    {
        ROS_ERROR("call service fail");
        return 1;
    }

    return 0;
}
